#include "scheduler.h"
#include "loader.h"
#include "process.h"
#include "os_io.h"
#include "semaphores.h"

pcb_t *g_process_ready_list[NUM_PROCESS_PRIORITIES] = {0};
pcb_t *g_running_process = NULL;
pcb_t *os_process = NULL;

void add_process_to_ready_queue(pcb_t *pcb_ptr)
{
	uint8_t process_priority = pcb_ptr->process_priority;
	pcb_t *head = g_process_ready_list[process_priority];

#ifdef NEW_PROCESS_AT_FRONT
	pcb_ptr->next_ready = head;
	pcb_ptr->prev_ready = pcb_ptr;
	if (head)
	{
		pcb_ptr->prev_ready = head->prev_ready;
		head->prev_ready = pcb_ptr;
	}
	g_process_ready_list[process_priority] = pcb_ptr;
	pcb_ptr->process_state = process_ready;
	return;
#endif
	pcb_ptr->next_ready = NULL;
	if (head)
	{
		head->prev_ready->next_ready = pcb_ptr;	// move to end of queue
		pcb_ptr->prev_ready = head->prev_ready;	// Update prev pointer
		head->prev_ready = pcb_ptr;
	}
	else
	{
		pcb_ptr->prev_ready = pcb_ptr;
		g_process_ready_list[process_priority] = pcb_ptr;
	}

	pcb_ptr->process_state = process_ready;
	//printf("Added 0x%x to ready queue 0x%x\n", (uint32_t)pcb_ptr, pcb_ptr->process_priority);
	return;
}

pcb_t * get_active_process(void)
{
	uint8_t cur_priority = 0;
	pcb_t *candidate = NULL;

	for (cur_priority = PROCESS_HIGHEST_PRIORITY; cur_priority <= PROCESS_LOWEST_PRIORITY; cur_priority++)
	{
		if (g_process_ready_list[cur_priority])
		{
			/*
			 * Current process shall continue to remain active if its 
			 * state is "running" and it has higher priority than the
			 * best candidate in the ready queue
			 */
			if (g_running_process->process_state == process_running) {
				if (g_running_process->process_priority <= cur_priority) {
					return g_running_process;
				}
			}

			candidate = g_process_ready_list[cur_priority];
			//candidate->prev_ready->next_ready = candidate->next_ready;

			if (candidate->next_ready)
				candidate->next_ready->prev_ready = candidate->prev_ready;

			g_process_ready_list[cur_priority] = candidate->next_ready;
		//	printf("Extracted 0x%x from ready queue 0x%x\n", candidate, cur_priority);
			break;
		}

	}

	if (!candidate) {
		if (g_running_process->process_state == process_running) {
			candidate = g_running_process;
		} else {
			printf("Error. No running process found. g_running_process:0x%x, state:0x%x\n",
			       (uint32_t)g_running_process, (uint32_t)g_running_process->process_state);
		//	printf("g_ready_list is: 0x%x\n", g_process_ready_list[15]);
			printf("OS process state: 0x%x\n", os_process->process_state);
			dump_processes();
			while (1);
		}
	} 

	return candidate;
}


void dump_regs(uint32_t *regs)
{
    uint8_t i = 0;
    for (i = 0; i < MAX_REGS; i++) {
        printf("0x%x ", regs[i]);
	if ((i & 0x3)== 0x0) {
	   printf("\n");
        }
    }
    return;
}

uint32_t *scheduler(uint32_t *register_set)
{
	pcb_t *candidate = NULL;
	pcb_t *old_process = g_running_process;

	//printf("Scheduler hit\n");

	candidate = get_active_process();

	if (candidate != g_running_process)
	{
	       //printf("scheduler: Switching context from 0x%x to 0x%x\n", (uint32_t)g_running_process, (uint32_t)candidate);
		if (g_running_process->process_state == process_dead) {
			free_process_memory(g_running_process);
			g_running_process = NULL;
		} else {
			uint8_t i = 0;
			for (i = 0; i < MAX_REGS; i++) {
				g_running_process->regs[i] = register_set[i];
			}
			//printf("Saved register set\n");
			//dump_regs(register_set);

			if (g_running_process->process_state == process_running)
			{
				add_process_to_ready_queue(g_running_process);
			}
		}

		candidate->process_state = process_running;
		g_running_process = candidate;
		//printf("scheduler: Switching back to the caller\n");
		//dump_regs(g_running_process->regs);

	} else {
	    //printf("scheduler: Resuming current process\n");
	}

	return (old_process == g_running_process) ? 0 : g_running_process->regs;
}

void block_process_on_sem(uint32_t data)
{
	sem_t *sem_ptr = (sem_t *)data;

	if (!g_running_process)
	{
		printf("Trying to block a process when no process is running!!\n");
		while (1);
	}

	g_running_process->process_state = process_waiting;
	//printf("%s blocked on sem\n", g_running_process->name);

	// Add process to sem_ptr's blocked queue
	sem_ptr->process_list.prev_process->next_process = &g_running_process->blocked_process;
	g_running_process->blocked_process.prev_process = sem_ptr->process_list.prev_process;
	g_running_process->blocked_process.next_process = &sem_ptr->process_list;
	sem_ptr->process_list.prev_process = &g_running_process->blocked_process;

	sem_ptr->sem_lock = SEM_LOCKED;

	return;
}

void release_process_from_sem(uint32_t data)
{
	sem_t *sem_ptr = (sem_t *)data;
	process_list_t *pr = NULL;

	pr = sem_ptr->process_list.next_process;

	sem_ptr->process_list.next_process = pr->next_process;
	pr->next_process->prev_process = pr->prev_process;

        if (sem_ptr->process_list.next_process == &sem_ptr->process_list) {
	    sem_ptr->sem_lock = SEM_UNLOCKED;
	}

	// add released process to ready queue
	pcb_t *process_ptr = NULL;
	process_ptr = (pcb_t *)((uint32_t)pr - (uint32_t)OFFSET_OF(blocked_process, pcb_t));
	add_process_to_ready_queue(process_ptr);

	return;
}

